
class PID():
    def __init__(self,kp = 1,kd = 1,ki = 0):
        self.kp = kp
        self.kd = kd
        self.ki = ki
        self.err = 0
        self.last_err = 0
        self.d_err = 0
        self.i_err = 0
        self.max_i_err = 100
        self.out_err = 0

    def reinit(self,kp,kd,ki):
        self.kp = kp
        self.kd = kd
        self.ki = ki
        self.err = 0
        self.last_err = 0
        self.d_err = 0
        self.i_err = 0
        self.out_err = 0

    def set_coef(self,kp,kd,ki):
        self.kp = kp
        self.kd = kd
        self.ki = ki

    def update_err(self,err):
        self.last_err = self.err
        self.err = err
        self.d_err = self.err - self.last_err

        if self.d_err == self.err:
            self.d_err = 0

        if self.i_err > self.max_i_err:
            self.i_err = self.max_i_err - 1
        elif self.i_err < -self.max_i_err:
            self.i_err = -self.max_i_err + 1
        elif err < 30:
            self.i_err += self.err
        else:
            self.i_err += self.err

        self.out_err = self.kp * self.err + self.kd * self.d_err + self.ki * self.i_err

        return self.out_err